#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp" //消息接口
#include "tf2/LinearMath/Quaternion.hpp" //提供tf2.Quaternion类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" //消息类型转换
#include "tf2_ros/transform_listener.hpp"//静态坐标广播器
#include "tf2_ros/buffer.hpp"//静态坐标广播器
#include "tf2/utils.hpp" //四元数转欧拉角
#include <chrono>
using namespace std::chrono_literals;

class TFListener : public rclcpp::Node
{
    private:
        std::shared_ptr<tf2_ros::TransformListener> listener_;
        std::shared_ptr<tf2_ros::Buffer> buffer_;
        rclcpp::TimerBase::SharedPtr timer_;
    public:
        TFListener(): Node("dynamic_tf_broadcaster")
        {
            this->buffer_=std::make_shared<tf2_ros::Buffer>(this->get_clock());
            this->listener_=std::make_shared<tf2_ros::TransformListener>(*buffer_,this);
            
            timer_=this->create_wall_timer(100ms,std::bind(&TFListener::listen_tf,this));
        }

        void listen_tf()
        {
            try
            {
                const auto tranforms=buffer_->lookupTransform("base_link","target_point",
                    this->get_clock()->now(),rclcpp::Duration::from_seconds(1));
                auto translation =tranforms.transform.translation;
                auto rotation =tranforms.transform.rotation;

                double y,p,r;
                tf2::getEulerYPR(rotation,y,p,r);
                RCLCPP_INFO(get_logger(),"平移：%f,%f,%f",translation.x,translation.y,translation.z);
                RCLCPP_INFO(get_logger(),"旋转：%f,%f,%f",y,p,r);
            }
            catch(const std::exception& e)
            {
               RCLCPP_WARN(get_logger(),"%s",e.what());
            }
            
            // geometry_msgs::msg::TransformStamped transform_;

           

        };


};

int main (int arg,char* argv[])
{
    rclcpp::init(arg,argv);
    auto nodo=std::make_shared<TFListener>();
    rclcpp::spin(nodo);
    rclcpp::shutdown();
    return 0;
}